

#pragma once
#ifndef TEST_PROJECT_RASP_TRACK_H
#define TEST_PROJECT_RASP_TRACK_H
#pragma pack(1)

//航迹跟踪模块使用说明

//1、SetRadarTime				设置当前时间				单位：秒
//2、SetRadarPerCircleTime		设置雷达扫描一圈的时间		单位：秒
//3、SetRgate					设置距离门				单位：米
//4、SetAzi						设置当前帧的方位角		单位：度
//5、SetEle						设置当前帧的俯仰角		单位：度
//6、SetUpdateCnt				设置起航点数
//7、SetAziThreshold				设置方位角门槛值			单位：度
//8、SetEleThreshold				设置俯仰角门槛值			单位：度
//9、SetRangeThreshold			设置距离门槛值			单位：米
//10、SetVelThreshold			设置速度门槛值			单位：米/秒
//11、SetIntervalTime			设置两帧时间间隔			单位：秒
//12、SetNetIPAddres				设置点迹发送目的的IP地址
//13、SetNetPort					设置点迹发送目的的端口号
//14、SetIsSendData				设置该模块是否发送航迹信息给目的地址
//7、DataProcess					DataIn，输入：内容为第一个int字节表示凝聚的点数N，后面跟着N个PointInfo结构体
//								DataOut,输出：无
//


#include <cassert>
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <string.h>
#include <vector>

#pragma comment(lib, "ws2_32.lib")

#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <sys/types.h>
#include <pthread.h>
#include <sys/ioctl.h>//for FIONBIO
#include <net/if.h>
#include <error.h>
#include <sys/wait.h>
#include <sys/time.h>
#include <unistd.h>
#include <semaphore.h>
#include <fcntl.h>
#include <ifaddrs.h>

#define MAX_FRAME_MEARS_NUM (1000)
#define MEAS_DIM 6
#define MAX_TRACK (1000)
#define M_PI 3.14159265358979323846
#define MR (0)
#define MAZ (1)
#define MEL (2)
#define MDR (3)
#define MEAS_NSTD_EL (1.5)
#define MEAS_NSTD_AZ (2.5)
#define MEAS_NSTD_R (100)  //原版
#define MEAS_NSTD_DR (10)
#define Q_R (10)
#define Q_AZ (0.01)
#define Q_EL (0.5)

#define MAX_RDOT (1000)
#define DEL_FRM_NUM (3.1)
#define MAX_R (10000)

struct TRACK_DATA {
    bool bUsed;
    unsigned int uTrackID;
    float PreMeas[MEAS_DIM];
    float fLastMeasTime;
    float fArfaBeta_K_R[2];
    float fArfaBeta_K_Az[2];
    float fArfaBeta_K_El[2];
    float fAz[2];
    float fR[2];
    float fEl[2];
    float fPreAz[2];
    float fPreR[2];
    float fPreEl[2];

    int iLastMeasID;
    int iUpdateCnt;
    float fInitTime;

};
struct ALL_TRACK_INFO {
    int iTrackNum;
    TRACK_DATA TracksData[MAX_TRACK];

};
struct FRAME_MEASURE {
    int iFrameNo;
    int iAntFrameNo;
    float fAz;
    float fEl;
    int iMeasNum;
    float fMeasures[MAX_FRAME_MEARS_NUM][MEAS_DIM];
    float fRadarTime;
    float fAntFrmTime;
};
struct MEAS_ASSOC_INFO {
    bool bUsed;
    int iAssocTrackIndex;
};
struct TRACKS_MEAS_ASSOC_RES {
    int iMeasNum;
    MEAS_ASSOC_INFO MeasAssocInfo[MAX_FRAME_MEARS_NUM];
};
struct RadarMeaInfo {
    float fRange;
    float fAz;
    float fEl;
    float fdR;
    float fMeasTime;
    float fbak[3];
};
struct RadarTrackInfo {
    float fRange;
    float fAz;
    float fEl;
    float fdR;
    float fTrackTime;
    int iTrackId;
    float fSpeed;
    float fHeadCourse;
};
struct RadarResultRecord {
    float fRange;
    float fAz;
    float fEl;
    float fdR;
    float iTrackId;
};
#pragma pack()

class Rasp_Track {
public:
    Rasp_Track();

    ~Rasp_Track();

    void UnInit();

    bool SetRadarTime(float Radar_time);

    bool SetRadarPerCircleTime(float Radar_PerCircle_time);

    bool SetRgate(float rgate);

    bool SetAzi(float Azi);

    bool SetEle(float Ele);

    bool SetUpdateCnt(int UpdateCnt);

    bool SetAziThreshold(float azithreshold);

    bool SetEleThreshold(float elethreshold);

    bool SetRangeThreshold(float rangethreshold);

    bool SetVelThreshold(float velthreshold);

    bool SetIntervalTime(float intervaltime);

    bool SetIsSendData(bool issend);

    int DataProcess(void *DataIn, int inLen, void *DataOut, int outLen);

private:
#pragma pack(1)
    struct PointInfo {
        double Range;//距离
        double Angle;//角度
        double Envelope;//包络幅度
        double time_weight;//临时
        double time;//时间
        double interrupt;//间隔，临时变量
        int joint_flag;//标志位，临时变量
        double Ele;
        double doppler;
    };
#pragma pack()
private:
    struct TrackSend {
        int Track_NO;
        float Time;
        float Range;
        float Azimuth;
        float Elevation;
        float Velocity;
        float Hight;
        char Backup[20] = {0};
    };

    float ResiExp(float fR, float fAz, float fEl, float fdR);

    void DataTransform(std::vector <PointInfo> &pointin, float rgate, FRAME_MEASURE *pFrameMeasure,
                       float Radar_PerCircle_time, float Radar_time, float Azi, float Ele);

    FRAME_MEASURE
    MeasRestrict(bool bRestrict, FRAME_MEASURE FrameMeasureIn, float fAngleMin, float fAngleMax, float fRMin,
                 float fRMax);

    void TrackPredict(float fMeasTime, ALL_TRACK_INFO *pAllTrackInfo);

    void UpdateGlobalVariableFilter(float fRadarTime, ALL_TRACK_INFO *pAllTrackInfo);

    void CalcArbFilterPara(float dT, float MeaNoise, float QNoise, float K[2]);

    void TrackUpdate(FRAME_MEASURE FrameMeas, TRACKS_MEAS_ASSOC_RES TracksMeasAssocRes, ALL_TRACK_INFO *pAllTrackInfo);

    void TrackFilterUpdate(float fResidual[MEAS_DIM], int iTrackIdx, ALL_TRACK_INFO *pAllTrackInfos);

    void AddNewTrack(TRACKS_MEAS_ASSOC_RES TracksMeasAssocRes, FRAME_MEASURE FrameMeas, ALL_TRACK_INFO *pAllTrackInfo);

    void TrackManage(float fRadarTime, float fAntFrmTime, ALL_TRACK_INFO *pAllTrackInfos);

    TRACKS_MEAS_ASSOC_RES CalcTracksAssocRes(FRAME_MEASURE FrameMeas, ALL_TRACK_INFO AllTrackInfo);

private:
    ALL_TRACK_INFO AllTrackInfo;
    int iUtag;
    float m_Radar_time;
    float m_Radar_PerCircle_time;
    float m_rgate;
    float m_Azi;//弧度
    float m_Ele;
    int m_UpdateCnt;

    float m_AziThreshold;
    float m_EleThreshold;
    float m_RangeThreshold;
    float m_VelThreshold;
    float m_IntervalTime;


    bool isSend;
};


#endif //TEST_PROJECT_RASP_TRACK_H
